
/**
 * @author Remi Destigny
 * Class used to store a quaternion and retrieve the euler angle from this quaternion in a 2D space
 */
public class Quaternion {
	
	private double q0;
	private double q1;
	private double q2;
	private double q3;
	
	/**
	 * Standard constructor
	 * @param q0
	 * @param q1
	 * @param q2
	 * @param q3
	 */
	public Quaternion(double q0, double q1, double q2, double q3) {
		super();
		this.q0 = q0;
		this.q1 = q1;
		this.q2 = q2;
		this.q3 = q3;
	}

	/**
	 * Compute the euler angle corresponding to the robot orientation 
	 * @return The euler angle given by the quaternion
	 */
	public double getPsi() {
		return Math.atan2(2*(q0*q3+q1*q2),
				1 - 2*(Math.pow(q2, 2) + Math.pow(q3, 2)));
	}
	
	
	public double getQ0() {
		return q0;
	}
	public void setQ0(double q0) {
		this.q0 = q0;
	}
	public double getQ1() {
		return q1;
	}
	public void setQ1(double q1) {
		this.q1 = q1;
	}
	public double getQ2() {
		return q2;
	}
	public void setQ2(double q2) {
		this.q2 = q2;
	}
	public double getQ3() {
		return q3;
	}
	public void setQ3(double q3) {
		this.q3 = q3;
	}
	
	
}
